/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package ticks.chromedome.subsystems;

import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.command.PIDCommand;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
import ticks.chromedome.RobotMap;
import ticks.chromedome.commands.SlashStop;

/**
 *
 * @author Jared
 */
public class SlashBarPID extends PIDSubsystem {

    private static final double Kp = 1.0;
    private static final double Ki = 0.1;
    private static final double Kd = 0.0;
    private SpeedController slash_motor;
    private AnalogChannel slash_pot;

    // Initialize your subsystem here
    public SlashBarPID() {
        super("SlashBarPID", Kp, Ki, Kd);
        slash_motor = new Victor(RobotMap.SLASH_MOTOR);
        slash_pot = new AnalogChannel(RobotMap.SLASH_POT_ANALOG);
       
        // Use these to get going:
        // setSetpoint() -  Sets where the PID controller should move the system
        //                  to
        // enable() - Enables the PID controller.
    }
    
    public void initDefaultCommand() {
        setDefaultCommand(new SlashStop());
       
    }
    public void setSlashSetpoint(double input){
        setSetpoint(input);
    }
    public void enableSlash(){
        enable();
    }
    public void disableSlash(){
        disable();
    }
    public void setSlashJog(double delta_setpoint){
        setSetpointRelative(delta_setpoint);
    }
    
    public boolean slashAtSetpoint(){
        return Math.abs(getPosition()-getSetpoint())<0.2;
    }
    
    protected double returnPIDInput() {
        
       return slash_pot.getAverageVoltage();
    }
    
    protected void usePIDOutput(double output) {
        slash_motor.set(output);
    }
}
